Integrated Navigation System Using Sigma-Point Kalman Filter and Particle Filter

The paper describes integration of Inertial Navigation System (INS) and Global Positioning System (GPS) navigation systems using new approaches for navigation information processing based on efficient SigmaPoint Kalman filtering and Particle filtering. The paper points out the inherent shortcomings in using the linearization techniques in standard Kalman filters (like Linearized Kalman filter or Extended Kalman filter) and presents, as an alternative, a family of improved derivativeless nonlinear filters. The integrated system was created in a simulation environment. An original contribution of the work consists in creation of models in the simulation environment to confirm the algorithms. The work results are represented in a chart and supported by statistical data to confirm the rightness of the algorithms developed. 1.0 INTRODUCTION Accurate and reliable navigation systems will have an important role for enhanced military capabilities in the coming years. The INS and GPS are widely used navigation systems in several applications. The main reason for their usage is their dimensions and weight, and their relatively simple implementation in the navigation system. Integrated navigation means that the outputs from two or more navigation sensors are blended to increase the overall accuracy and reliability of the navigation system. Due to its reliability, autonomy and short-term accuracy, inertial navigation is usually regarded as the primary source of navigation data. The major drawback of inertial navigation is that initialization and sensor errors cause the computed quantities to drift. To stabilize the drift and ensure long-term accuracy, the inertial navigation system is integrated with one or more aiding sources. Nowadays, the GPS is the standard aiding source. Although, satellite navigation has a widespread use, problems with the GPS such as reception limitation and interference increase the relevance of other aiding navigation sensors. The main objective of the INS/GPS integration is to merge information from INS and GPS sensors and provide estimates of the states of the vehicle with greater accuracy than relying on the information from the individual sensors. For many years loose and tightly coupled schemes have been used to provide robust solution. These solutions were used in many applications as in automotive, aerospace robotics and other systems where there are needs for precise navigation. The inertial navigation is based on measurements of vehicle specific forces and rotation rates obtained from on-board instrumentation consisting of triads of gyros and accelerometers that create an IMU (Inertial Measurement Unit). The measurements from the IMU are used for determination of the vehicle position, velocity and attitude using Newton’s equations of motion in the navigation computer. The velocity and the position vectors are computed by double integration of the sum of the gravitational and Sotak, M.; Sopata, M.; Berezny, S. (2007) Integrated Navigation System Using Sigma-Point Kalman Filter and Particle Filter. In Military Capabilities Enabled by Advances in Navigation Sensors (pp. 27-1 – 27-12). Meeting Proceedings RTO-MP-SET-104, Paper 27. Neuillysur-Seine, France: RTO. Available from: http://www.rto.nato.int. Integrated Navigation System Using Sigma-Point Kalman Filter and Particle Filter 27 2 RTO-MP-SET-104 UNCLASSIFIED/UNLIMITED UNCLASSIFIED/UNLIMITED the nongravitational accelerations from the accelerometers and the orientation in space is determined by integrating the rotation rates obtained from the three gyros. The INS may be mechanized in either gimbaled or strapdown configurations. Gimbaled system is usually heavier and more expensive than a strapdown system and that is the reason why a strapdown INS is used for UAVs or other systems where the weight, size and cost play a significant role. Though INS is autonomous and provides good short-term accuracy, its usage as a stand-alone navigation system is limited due to the time-dependent growth of the inertial sensor errors that is the main disadvantage of using the INS. The accuracy of the INS is therefore highly dependent on the sensor quality, navigation system mechanization and dynamics of the flight vehicle. The GPS is a space based radio navigation system. This system can provide high accuracy positioning anytime and anywhere in the world. The main disadvantage of the GPS system is that the system is not self-contained and autonomous. Accuracy of the GPS system depends on many factors, for instance receiver clock bias, bias due to receiver clock drift, bias due to system clock error, ionospheric delay, tropospheric delay, random noise, etc. However, compared to the INS system, the GPS receiver is low frequency sensor with bounded errors, thus providing the state information at low update rates with nonincreasing errors with time. Inertial Navigation System High position and velocity accuracy over short term Accuracy decreasing with time Affected by gravity High measurement output rate Autonomous Global Positioning System High position and velocity accuracy over long term Uniform accuracy, independent of time Not sensitive to gravity Non-autonomous Low measurement output date Integrated INS and GPS system High position and velocity accuracy over long term High data rate Navigation output during GPS signal outages Precise attitude determination Integrated Navigation System Using Sigma-Point Kalman Filter and Particle Filter RTO-MP-SET-104 27 3 UNCLASSIFIED/UNLIMITED UNCLASSIFIED/UNLIMITED Figure 1: Errors of navigation sensors 2.0 NONLINEAR FILTERING The nonlinear filtering problem consists of estimating the states of a nonlinear stochastic dynamical system. The class of systems considered is broad and includes bit/attitude estimation, integrated navigation, and radar or sonar surveillance systems. Because most of these systems are nonlinear and/or non-Gaussian, a significant challenge to engineers and scientists is to find efficient methods for on-line, real-time estimation and prediction of the system states and error statistics from sequential observations. In a broad sense, general approaches to optimal nonlinear filtering can be described by a unified way using the recursive Bayesian approach. The central idea of this recursive Bayesian estimation is to determine the probability density function (PDF) of the state vector of the nonlinear systems conditioned on the available measurements. This posterior density function provides the most complete description of the state estimate of the systems. In linear systems with Gaussian process and measurement noises, an optimal closed-form solution is the well-known Kalman filter. In nonlinear systems the optimal exact solution to the recursive Bayesian filtering problem is intractable since it requires infinite dimensional processes. Therefore, approximate nonlinear filters have been proposed. These approximate nonlinear filters can be categorized into five types [13]: 1) analytical approximations, 2) direct numerical approximations, 3) sampling-based approaches, 4) Gaussian mixture filters, 5) simulation-based filters. The most widely used approximate nonlinear filters are the Linearized Kalman filter (LKF) and Extended Kalman filter (EKF) that are representative analytical approximate nonlinear filters. The Kalman filter is used as a tool for stochastic estimation from noisy measurements. The Kalman filter is essentially a set of mathematical equations that implement a predictor-corrector type estimator that is optimal in the sense that it minimizes the estimated error covariance, when some presumed conditions are met. The EKF is similar to the LKF, but with a few differences. The main difference is that the linearization is performed around a trajectory estimated by the filter, not a pre-computed nominal one as in the LKF. Although the EKF maintains the elegant and computationally efficient recursive update form of the KF, it suffers a number of serious limitations. One of these limitations is that the covariance propagation and update are analytically linearized up to the first-order in the Taylor series expansion, and this suggests that Integrated Navigation System Using Sigma-Point Kalman Filter and Particle Filter 27 4 RTO-MP-SET-104 UNCLASSIFIED/UNLIMITED UNCLASSIFIED/UNLIMITED the region of stability may be small since nonlinearities in the system dynamics are not fully accounted for. Consequently, these approximations can introduce large errors in the true mean and covariance. Comparing the Kalman filtering with other methods of nonlinear filtering, the Kalman filter has a number of practical benefits. For example, there is a successful compromise between computational complexity and flexibility, the mean and covariance are linearly transformable, and the mean and covariance estimates can be used to characterize additional features of the distribution, e.g. significant modes [12]. As was mentioned above, the LKF and EKF simply linearize all nonlinear transformations and substitute Jacobian matrices for the linear transformations in the Kalman filter equations, but these procedures are accompanied by some shortcomings: linearized approximation can be extremely poor in cases when error propagation can’t be well approximated by a linear function, linearization can be applied only if the Jacobian matrices exist or in some situations calculation of Jacobian matrices is a very difficult and errorprone process. Based on these reasons different approaches to nonlinear filtering were developed. In this paper the Sigma-point Kalman filter (SPKF) and Particle filters (PF) are described. These filters belong to the simulation-based category of filters and they will be discussed in more detail in the next two sections. 3.0 PARTICLE FILTERING Numerical methods known as Monte Carlo methods can be described as statistical simulation methods, where statistical simulation is defined as a method that utilizes sequences of random numbers to perform the simulation. Despite the fact that Monte Carlo methods are known for such a long time only nowadays has progress in technique allowed us to apply these methods to complex applications. Monte Carlo methods are now used routinely in many diverse fields from the simulation of complex physical phenomena. The sequential Monte Carlo approach is known as the bootstrap filtering, the condensation algorithm, and the particle filtering [6]. Particle filters are simulation-based filtering methods where realizations (samples) of the state vector are produced to obtain an empirical approximation of the joint posterior distribution. In fact, particle filters are "tracking" a variable of interest as it evolves over time, typically with a non-Gaussian probability density function. In particle filters the probability density function is calculated using a likelihood function. For this reason multiple copies (particles) of the variable of interest are used, each with a specific weight and the variable of interest is then obtained by the weighted sum of all the particles, in other words, the normalized importance weight and corresponding particles constitute an approximation of the filtering density [15]. The particle filter is recursive (similarly to LKF and EKF) and operates in two phases: prediction and update. That means that after each operation, each particle is modified according to the variable of interest then its weight is recalculated and particles with small weights are rejected (this process is called resampling). Particle filter implementation can be described by the following algorithm: 1. Initialization: Generate ( ) ( ) i 0 0 p x x ∼ , i=1,....,N sample of the state vector is referred to as a particle. 2. Measurement update: Update the weights by the likelihood ( ) ( ) ( ) *( ) ( ) ( ) ( ) ( ) | t i i i i i k k 1 k k k 1 k k p p h i=1,....,N − − = ⋅ = ⋅ − v w w y x w y x Integrated Navigation System Using Sigma-Point Kalman Filter and Particle Filter RTO-MP-SET-104 27 5 UNCLASSIFIED/UNLIMITED UNCLASSIFIED/UNLIMITED Calculate likelihood by ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) dim( ) exp k k T i i 1 i k k k k k k 1 1 p h h h 2 2π −   − = ⋅ − − ⋅ ⋅ −     v y y x y x R y x R and normalize to *( ) ( ) *( ) i i k k N i k i 1 = = ∑ w w w 3. Resampling: Replicate particles in proportion to their weights [2]. Only resample when the effective number of samples is less than a threshold threshold N . ( ) ( ) eff threshold eff N 2 i k i 1 1 N N , 1 N N